#include "../include/obstacle.hpp"

GenObstacle::GenObstacle(/* args */)
{
    this->noise = 2.0;                         // 障碍物位置扰动
    this->obs_scale = 1.0;                     // 障碍物大小
    // 这里生成五个障碍物
    obs_index = {120, 160, 240, 320};
    // obs_index = {100, 200, 300, 500};
}

GenObstacle::~GenObstacle()
{
}

/**
 * 订阅 global_path 的回调函数
 * 在 global_path 上根据提前设置的障碍物下标添加障碍物
*/
void GenObstacle::global_path_callback(const nav_msgs::Path &msg) {
    vector<Location> obs_vec;
    vector<ReferencePoint> global_path;
    for (int i = 0; i < msg.poses.size(); i++) {
        ReferencePoint point;
        point.x = msg.poses[i].pose.position.x;
        point.y = msg.poses[i].pose.position.y;
        global_path.push_back(point);
    }
    // 然后在全局路径上选取几个坐标，作为障碍物的位置
    ros::Time cur_time = ros::Time::now();
    visualization_msgs::Marker obs_marker;
    obs_marker.header.frame_id = "world";
    obs_marker.header.stamp = cur_time;
    obs_marker.ns = "obstacle";
    obs_marker.type = visualization_msgs::Marker::POINTS;
    obs_marker.action = visualization_msgs::Marker::ADD;
    obs_marker.id = 0;

    obs_marker.scale.x = obs_scale;
    obs_marker.scale.y = obs_scale;
    obs_marker.color.r = 1.0;
    obs_marker.color.a = 1.0;

    // 发布静态障碍物
    for (int i = 0; i < obs_index.size(); i++) {
        // 生成障碍物位置信息
        Location obs;
        obs.x = global_path[obs_index[i]].x;
        obs.y = global_path[obs_index[i]].y;
        if (i % 2 == 0) obs.y += noise;
        else obs.x += noise;
        obs_vec.push_back(obs); 
        // 配置障碍物 point 的位置
        geometry_msgs::Point p;
        p.x = global_path[obs_index[i]].x;
        p.y = global_path[obs_index[i]].y;
        p.z = 1.0;
        obs_marker.points.push_back(p);
    }

    // 发布动态障碍物
    // 动态障碍物运动范围： (55, 60) - (65, 60) 两点之间以匀速周期往返
    int begin_x = 55;
    int end_x = 65;
    Location obs;
    obs.x = begin_x + obs_speed * index_time;
    obs.y = 60;
<<<<<<< HEAD
=======
    obs.speed = obs_speed * 10;
>>>>>>> dev
    obs_vec.push_back(obs);
    // 配置障碍物 point 的位置
    geometry_msgs::Point p;
    p.x = obs.x;
    p.y = obs.y;
<<<<<<< HEAD
    obs.speed = obs_speed * 10;
=======
>>>>>>> dev
    p.z = 1.0;
    obs_marker.points.push_back(p);
    if (isToRight) {
        index_time++;
        if (index_time >= 50) {
            // 如果障碍物移动到最右边，将其反向移动
            isToRight = false;
        }
    } else {
        index_time--;
        if (index_time <= 0) isToRight = true;
    }
    

    this->obs_pub.publish(obs_marker);
    this->obstacle_vec.assign(obs_vec.begin(), obs_vec.end());
}


vector<Location> GenObstacle::getObstacle(Location &host) {
    /**
     * 方法1：使用车辆到障碍物的向量点乘车辆的切向量和法向量作为纵向距离和横向距离，然后判断
     * 范围：返回车辆纵向 [-5, 40] 横向 [-5, 5]m 范围内的障碍物
     * 问题：对于曲率较大的道路，该方法盲区比较大
    */
    // vector<Location> obs_final;
    // Eigen::Vector2d host_tor(cos(host.heading), sin(host.heading));
    // Eigen::Vector2d host_nor(-sin(host.heading), cos(host.heading));
    // Eigen::Vector2d host_v(host.x, host.y);
    // vector<Location>::iterator it = this->obstacle_vec.begin();
    // while (it != this->obstacle_vec.end())
    // {
    //     Eigen::Vector2d obs_v(it->x, it->y);
    //     Eigen::Vector2d dis_v = obs_v - host_v;
    //     // 纵向距离
    //     double lon_dis = dis_v.dot(host_tor);
    //     double lat_dis = dis_v.dot(host_nor);
    //     if (lon_dis < 40 && lon_dis > -5 && lat_dis < 5 && lat_dis > -5) {
    //         obs_final.push_back(*it);
    //     }
    //     it++;
    // }
    /**
     * 方法2：使用小车到障碍物的向量点乘小车的切向量，判断障碍物在小车的前方还是后方
     *      然后计算小车到障碍物的距离。如果障碍物在前方，则范围为50表示选取50米内的目标
     *      如果在小车后面，则选取距离5米内的障碍物
    */
    vector<Location> obs_final;
    Eigen::Vector2d host_tor(cos(host.heading), sin(host.heading));
    Eigen::Vector2d host_v(host.x, host.y);
    vector<Location>::iterator it = this->obstacle_vec.begin();
    while (it != this->obstacle_vec.end()) {
        Eigen::Vector2d obs_v(it->x, it->y);
        Eigen::Vector2d dis_v = obs_v - host_v;
        double res = dis_v.dot(host_tor);
        double dis = sqrt(pow(host.x - it->x, 2) + pow(host.y - it->y, 2));
        if (res >= 0) {     // 障碍物在车的前方，向前探测60米
            if (dis <= 60)
                obs_final.push_back(*it);
        } else {
            if (dis <= 5)   // 障碍物在车的后面，向后探测5米
                obs_final.push_back(*it);
        }
        it++;
    }
    return obs_final;
}


void GenObstacle::run() {
    global_path_sub = nh.subscribe("/global_path_center", 1, &GenObstacle::global_path_callback, this);
    obs_pub = nh.advertise<visualization_msgs::Marker>("trajectory_marker", 10);
    // ros::spin();
}


int basic_shape() {
    ros::NodeHandle nh;
    ros::Rate r(10);
    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
    uint32_t shape = visualization_msgs::Marker::CUBE;
    

    while (ros::ok())
    {
        visualization_msgs::Marker marker;
        //全局坐标系的名称
        marker.header.frame_id = "my_frame";
        marker.header.stamp = ros::Time::now();
        //命名空间
        marker.ns = "basic_shapes";
        marker.id = 0;
        marker.type = shape;
        //属性
        marker.action = visualization_msgs::Marker::ADD;
				// 被添加物体的位置
        marker.pose.position.x = 0;
        marker.pose.position.y = 0;
        marker.pose.position.z = 0;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;
				// 被添加物体的大小
        marker.scale.x = 1.0;
        marker.scale.y = 1.0;
        marker.scale.z = 1.0;
			 //  物体的颜色设置
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;
        marker.color.a = 1.0;
        // 物体的生存时间，这里是永远都存在
        marker.lifetime = ros::Duration();

        while (marker_pub.getNumSubscribers() < 1)
        {
            if (!ros::ok())
            {
                return 0;
            }
            ROS_WARN_ONCE("Please create a subscriber to the marker");
            sleep(1);
        }
        // 将上面的物体发布出去
        marker_pub.publish(marker);
        switch (shape)
        {
        case visualization_msgs::Marker::CUBE:
            shape = visualization_msgs::Marker::SPHERE;
            sleep(2);
            break;
        case visualization_msgs::Marker::SPHERE:
            shape = visualization_msgs::Marker::ARROW;
            sleep(2);
            break;
        case visualization_msgs::Marker::ARROW:
            shape = visualization_msgs::Marker::CYLINDER;
            sleep(2);
            break;
        case visualization_msgs::Marker::CYLINDER:
            shape = visualization_msgs::Marker::CUBE;
            sleep(2);
            break;
        default:
            break;
        }
        r.sleep();

    }
    return 0;
}

void line_shape() {
    ros::NodeHandle n;
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("trajectory_marker", 10);

    ros::Rate r(30);

    float f = 0.0;
    while (ros::ok())
    {

        visualization_msgs::Marker points, line_strip, line_list;
        points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "world";
        points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
        points.ns = line_strip.ns = line_list.ns = "points_and_lines";
        points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
        points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;



        points.id = 0;
        line_strip.id = 1;
        line_list.id = 2;



        points.type = visualization_msgs::Marker::POINTS;
        line_strip.type = visualization_msgs::Marker::LINE_STRIP;
        line_list.type = visualization_msgs::Marker::LINE_LIST;



        // POINTS markers use x and y scale for width/height respectively
        points.scale.x = 0.2;
        points.scale.y = 0.2;

        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
        line_strip.scale.x = 0.1;
        line_list.scale.x = 0.1;



        // Points are green
        points.color.g = 1.0f;
        points.color.a = 1.0;

        // Line strip is blue
        line_strip.color.b = 1.0;
        line_strip.color.a = 1.0;

        // Line list is red
        line_list.color.r = 1.0;
        line_list.color.a = 1.0;



        // Create the vertices for the points and lines
        for (uint32_t i = 0; i < 100; ++i)
        {
            float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
            float z = 5 * cos(f + i / 100.0f * 2 * M_PI);

            geometry_msgs::Point p;
            p.x = (int32_t)i - 50;
            p.y = y;
            p.z = z;

            points.points.push_back(p);
            line_strip.points.push_back(p);

            // The line list needs two points for each line
            line_list.points.push_back(p);
            p.z += 1.0;
            line_list.points.push_back(p);
        }


        marker_pub.publish(points);
        marker_pub.publish(line_strip);
        marker_pub.publish(line_list);

        r.sleep();

        f += 0.04;
    }
}

// int main(int argc,char** argv)
// {
//     ros::init(argc, argv, "points_and_lines");
//     line_shape();
//     return 0;
// }
